#include <linux/numa.h>
#include <linux/errno.h>
#include <linux/mm.h>

#include <asm/page.h>
#include <asm/setup.h>
#include <asm/memory.h>
#include <asm/system.h>
#include <asm/cachetype.h>
#include <asm/cputype.h>

unsigned int processor_id = 0;
unsigned int __machine_arch_type = 0;
unsigned int __atags_pointer = 0;
unsigned int cr_alignment = 0;

unsigned int cacheid;
static const char *cpu_name = "default";

struct stack {
	u32 irq[3];
	u32 abt[3];
	u32 und[3];
} ____cacheline_aligned;

static struct stack stacks[NR_CPUS];

static const char *proc_arch[] = {
	"undefined/unknown",
	"3",
	"4",
	"4T",
	"5",
	"5T",
	"5TE",
	"5TEJ",
	"6TEJ",
	"7",
	"?(11)",
	"?(12)",
	"?(13)",
	"?(14)",
	"?(15)",
	"?(16)",
	"?(17)",
};

int cpu_architecture(void)
{
	int cpu_arch;

	printf("this is %s(): %d\r\n", __func__, __LINE__);
	printf("this is %s(): %d >>> read_cpuid_id() = 0x%x\r\n", __func__, __LINE__, read_cpuid_id());

	if ((read_cpuid_id() & 0x0008f000) == 0) {
		printf("this is %s(): %d >>> read_cpuid_id() = 0x%x\r\n", __func__, __LINE__, read_cpuid_id());
		cpu_arch = CPU_ARCH_UNKNOWN;
	} else if ((read_cpuid_id() & 0x0008f000) == 0x00007000) {
		printf("this is %s(): %d >>> read_cpuid_id() = 0x%x\r\n", __func__, __LINE__, read_cpuid_id());
		cpu_arch = (read_cpuid_id() & (1 << 23)) ? CPU_ARCH_ARMv4T : CPU_ARCH_ARMv3;
	} else if ((read_cpuid_id() & 0x00080000) == 0x00000000) {
		printf("this is %s(): %d >>> read_cpuid_id() = 0x%x\r\n", __func__, __LINE__, read_cpuid_id());
		cpu_arch = (read_cpuid_id() >> 16) & 7;
		if (cpu_arch)
			cpu_arch += CPU_ARCH_ARMv3;
	} else if ((read_cpuid_id() & 0x000f0000) == 0x000f0000) {
		printf("this is %s(): %d >>> read_cpuid_id() = 0x%x\r\n", __func__, __LINE__, read_cpuid_id());
		unsigned int mmfr0;

		/* Revised CPUID format. Read the Memory Model Feature
		 * Register 0 and check for VMSAv7 or PMSAv7 */
		asm("mrc	p15, 0, %0, c0, c1, 4"
		    : "=r" (mmfr0));
		if ((mmfr0 & 0x0000000f) == 0x00000003 ||
		    (mmfr0 & 0x000000f0) == 0x00000030)
			cpu_arch = CPU_ARCH_ARMv7;
		else if ((mmfr0 & 0x0000000f) == 0x00000002 ||
			 (mmfr0 & 0x000000f0) == 0x00000020)
			cpu_arch = CPU_ARCH_ARMv6;
		else
			cpu_arch = CPU_ARCH_UNKNOWN;
	} else
		cpu_arch = CPU_ARCH_UNKNOWN;

	printf("this is %s(): %d >>> read_cpuid_id() = 0x%x\r\n", __func__, __LINE__, read_cpuid_id());
	printf("this is %s(): %d >>> cpu_arch = %d\r\n", __func__, __LINE__, cpu_arch);

	return cpu_arch;
}

static void cacheid_init(void)
{
	unsigned int cachetype = read_cpuid_cachetype();
	unsigned int arch = cpu_architecture();

	if (arch >= CPU_ARCH_ARMv6) {
		if ((cachetype & (7 << 29)) == 4 << 29) {
			/* ARMv7 register format */
			cacheid = CACHEID_VIPT_NONALIASING;
			if ((cachetype & (3 << 14)) == 1 << 14)
				cacheid |= CACHEID_ASID_TAGGED;
		} else if (cachetype & (1 << 23))
			cacheid = CACHEID_VIPT_ALIASING;
		else
			cacheid = CACHEID_VIPT_NONALIASING;
	} else {
		cacheid = CACHEID_VIVT;
	}

	printf("CPU: %s data cache, %s instruction cache\r\n",
		cache_is_vivt() ? "VIVT" :
		cache_is_vipt_aliasing() ? "VIPT aliasing" :
		cache_is_vipt_nonaliasing() ? "VIPT nonaliasing" : "unknown",
		cache_is_vivt() ? "VIVT" :
		icache_is_vivt_asid_tagged() ? "VIVT ASID tagged" :
		cache_is_vipt_aliasing() ? "VIPT aliasing" :
		cache_is_vipt_nonaliasing() ? "VIPT nonaliasing" : "unknown");
}

static void setup_processor(void)
{
// 	struct proc_info_list *list;

// 	/*
// 	 * locate processor in the list of supported processor
// 	 * types.  The linker builds this table for us from the
// 	 * entries in arch/arm/mm/proc-*.S
// 	 */
// 	list = lookup_processor_type(read_cpuid_id());
// 	if (!list) {
// 		printk("CPU configuration botched (ID %08x), unable "
// 		       "to continue.\n", read_cpuid_id());
// 		while (1);
// 	}

// 	cpu_name = list->cpu_name;

// #ifdef MULTI_CPU
// 	processor = *list->proc;
// #endif
// #ifdef MULTI_TLB
// 	cpu_tlb = *list->tlb;
// #endif
// #ifdef MULTI_USER
// 	cpu_user = *list->user;
// #endif
// #ifdef MULTI_CACHE
// 	cpu_cache = *list->cache;
// #endif

	printf("CPU: %s [%08x] revision %d (ARMv%s), cr=%08lx\r\n",
	       cpu_name, read_cpuid_id(), read_cpuid_id() & 15,
	       proc_arch[cpu_architecture()], cr_alignment);

// 	sprintf(init_utsname()->machine, "%s%c", list->arch_name, ENDIANNESS);
// 	sprintf(elf_platform, "%s%c", list->elf_name, ENDIANNESS);
// 	elf_hwcap = list->elf_hwcap;
// #ifndef CONFIG_ARM_THUMB
// 	elf_hwcap &= ~HWCAP_THUMB;
// #endif

	cacheid_init();
	// cpu_proc_init();
}

void cpu_init(void)
{
	// unsigned int cpu = smp_processor_id();
	unsigned int cpu = 0;
	struct stack *stk = &stacks[cpu];

	// if (cpu >= NR_CPUS) {
	// 	printk(KERN_CRIT "CPU%u: bad primary CPU number\n", cpu);
	// 	BUG();
	// }

	/*
	 * Define the placement constraint for the inline asm directive below.
	 * In Thumb-2, msr with an immediate value is not allowed.
	 */
// #ifdef CONFIG_THUMB2_KERNEL
#define PLC	"r"
// #else
// #define PLC	"I"
// #endif

	/*
	 * setup stacks for re-entrant exception handlers
	 */
	__asm__ (
	"msr	cpsr_c, %1\n\t"
	"add	r14, %0, %2\n\t"
	"mov	sp, r14\n\t"
	"msr	cpsr_c, %3\n\t"
	"add	r14, %0, %4\n\t"
	"mov	sp, r14\n\t"
	"msr	cpsr_c, %5\n\t"
	"add	r14, %0, %6\n\t"
	"mov	sp, r14\n\t"
	"msr	cpsr_c, %7"
	    :
	    : "r" (stk),
	      PLC (PSR_F_BIT | PSR_I_BIT | IRQ_MODE),
	      "I" (offsetof(struct stack, irq[0])),
	      PLC (PSR_F_BIT | PSR_I_BIT | ABT_MODE),
	      "I" (offsetof(struct stack, abt[0])),
	      PLC (PSR_F_BIT | PSR_I_BIT | UND_MODE),
	      "I" (offsetof(struct stack, und[0])),
	      PLC (PSR_F_BIT | PSR_I_BIT | SVC_MODE)
	    : "r14");
}

static int arm_add_memory(unsigned long start, unsigned long size)
{
	struct membank *bank = &meminfo.bank[meminfo.nr_banks];

	if (meminfo.nr_banks >= NR_BANKS) {
		printf("NR_BANKS too low, "
			"ignoring memory at %#lx\r\n", start);
		return -EINVAL;
	}

	/*
	 * Ensure that start/size are aligned to a page boundary.
	 * Size is appropriately rounded down, start is rounded up.
	 */
	size -= start & ~PAGE_MASK;
	bank->start = PAGE_ALIGN(start);
	bank->size  = size & PAGE_MASK;
	bank->node  = PHYS_TO_NID(start);

	/*
	 * Check whether this memory region has non-zero size or
	 * invalid node number.
	 */
	if (bank->size == 0 || bank->node >= MAX_NUMNODES)
		return -EINVAL;

	meminfo.nr_banks++;
	return 0;
}

void setup_arch()
{
	setup_processor();

	meminfo.nr_banks = 0;

	arm_add_memory(0x80000000, 0x9FE00000 - 0x80000000);	// size 510MB
	// arm_add_memory(0x80000000, 0x3200000);	// size 50MB

	printf("this is %s(): %d\r\n", __func__, __LINE__);
	paging_init();

	printf("this is %s(): %d\r\n", __func__, __LINE__);
	cpu_init();

	printf("this is %s(): %d\r\n", __func__, __LINE__);
	early_trap_init();
}
